#pragma once
#include "elisa/elisa.h"
#include <iostream>
#include <sstream>
#include <algorithm>
#include <string>
#include <vector>
#include <gaol/gaol>

#include "Modeler.h"

#ifdef ELISA_PERFORMER
#include "elisa_chrono.h"
#endif

using namespace std;
using namespace elisa;

/* Problem: kinematics 2

@Article{MorganSommese1987,
 author    = {A. Morgan and A. Sommese},
 title     = {Computing all solutions to polynomial systems
              using homotopy continuation},
 journal   = {Appl. Math. Comput.},
 year      = {1987},
 volume    = {24},
 pages     = {115--138}
}

@book{Numerica1997,
author    = {P. {Van Hentenryck} and L. Michel and Y. Deville},
title     = {{N}umerica: a {M}odeling {L}anguage for {G}lobal {O}ptimization},
publisher = {MIT Press},
year      = {1997}
}
*/
class Kinematics2 : public Modeler
{
private:
	Model _model;

	RealConstant a11, a12, a13, a14;
	RealConstant a21, a22, a23, a24;
	RealConstant a31, a32, a33, a34;
	RealConstant a41, a42, a43, a44;
	RealConstant a51, a52, a53, a54;
	RealConstant a61, a62, a63, a64;
	RealConstant a71, a72, a73, a74;
	RealConstant a81, a82, a83, a84;
	RealConstant a91, a92, a93, a94;
	RealConstant a101, a102, a103, a104;
	RealConstant a111, a112, a113, a114;
	RealConstant a121, a122, a123, a124;
	RealConstant a131, a132, a133, a134;
	RealConstant a141, a142, a143, a144;
	RealConstant a151, a152, a153, a154;
	RealConstant a161, a162, a163, a164;
	RealConstant a171, a172, a173, a174;
	RealVar x1, x2, x3, x4, x5, x6, x7, x8;
	RealConstraint c1, c2, c3, c4, c5, c6, c7, c8;

public:
	Kinematics2()
		: x1(Interval(-1,1), "x1"),
		  x2(Interval(-1,1), "x2"),
		  x3(Interval(-1,1), "x3"),
		  x4(Interval(-1,1), "x4"),
		  x5(Interval(-1,1), "x5"),
		  x6(Interval(-1,1), "x6"),
		  x7(Interval(-1,1), "x7"),
		  x8(Interval(-1,1), "x8"),
		  a11(-0.24915068),   a12(0.12501635),    a13(-0.63555007),   a14(1.4894773),
		  a21(1.6091354),     a22(-0.68660736),   a23(-0.11571992),   a24(0.23062341),
		  a31(0.27942343),    a32(-0.11922812),   a33(-0.66640448),   a34(1.3281073),
		  a41(1.4348016),     a42(-0.71994047),   a43(0.11036211),    a44(-0.25864503),
		  a51(0.0),           a52(-0.43241927),   a53(0.29070203),    a54(1.1651720),
		  a61(0.40026384),    a62(0.0),           a63(1.2587767),     a64(-0.26908494),
		  a71(-0.80052768),   a72(0.0),           a73(-0.62938836),   a74(0.53816987),
		  a81(0.0),           a82(-0.86483855),   a83(0.58140406),    a84(0.58258598),
		  a91(0.074052388),   a92(-0.037157270),  a93(0.19594662),    a94(-0.20816985),
		  a101(-0.083050031), a102(0.035436896),  a103(-1.2280342),   a104(2.6868320),
		  a111(-0.38615961),  a112(0.085383482),  a113(0.0),          a114(-0.69910317),
		  a121(-0.75526603),  a122(0.0),          a123(-0.079034221), a124(0.35744413),
		  a131(0.50420168),   a132(-0.039251967), a133(0.026387877),  a134(1.2499117),
		  a141(-1.0916287),   a142(0.0),          a143(-0.057131430), a144(1.4677360),
		  a151(0.0),          a152(-0.43241927),  a153(-1.1628081),   a154(1.1651720),
		  a161(0.049207290),  a162(0.0),          a163(1.2587767),    a164(1.0763397),
		  a171(0.049207290),  a172(0.013873010),  a173(2.1625750),    a174(-0.69686809),
		  c1(pow(x1,2) + pow(x2,2) == 1),
		  c2(pow(x3,2) + pow(x4,2) == 1),
		  c3(pow(x5,2) + pow(x6,2) == 1),
		  c4(pow(x7,2) + pow(x8,2) == 1),
		  c5(x3*(a11*x1 + a31*x2 + a111) + x4*(a21*x1 + a41*x2 + a121) +
		     x8*(a61*x5 + a161) + x6*(a71*x7 + a141) + a91*x1 + a101*x2 + a131*x5 + a171 == 0),
		  c6(x3*(a12*x1 + a32*x2 + a112) + x4*(a22*x1 + a42*x2) +
		     x7*(a52*x5 + a152) + a92*x1  + a102*x2 + a132*x5 + a82*x6*x8 + a172 == 0),
		  c7(x3*(a13*x1 + a33*x2) + x4*(a23*x1 + a43*x2 + a123) + x7*(a53*x5 + a73*x6 + a153) +
		     x8*(a63*x5 + a83*x6 + a163) + a93*x1 + a103*x2 + a133*x5 + a143*x6 + a173 == 0),
		  c8(x3*(a14*x1 + a34*x2 + a114) + x4*(a24*x1 + a44*x2 + a124) + x7*(a54*x5 + a74*x6 + a154) +
		     x8*(a64*x5 + a84*x6 + a164) + a94*x1 + a104*x2 + a134*x5 + a144*x6 + a174 == 0),
		  _model()
	{
	}

	elisa::Model & model()
	{
		_model = c1 && c2 && c3 && c4 && c5 && c6 && c7 && c8;
		return _model;
	}

	void print()
	{
		cout << '\t' << x1.getName() << " = " << x1.domain() << endl;
		cout << '\t' << x2.getName() << " = " << x2.domain() << endl;
		cout << '\t' << x3.getName() << " = " << x3.domain() << endl;
		cout << '\t' << x4.getName() << " = " << x4.domain() << endl;
		cout << '\t' << x5.getName() << " = " << x5.domain() << endl;
		cout << '\t' << x6.getName() << " = " << x6.domain() << endl;
		cout << '\t' << x7.getName() << " = " << x7.domain() << endl;
		cout << '\t' << x8.getName() << " = " << x8.domain() << endl;
	}

	RealVar & varX()
	{
		return x1;
	}
	RealVar & varY()
	{
		return x2;
	}
};
